#include "utils.h"
#include "common/node.h"
namespace control
{

TrajectoryTrackingTruck::TrajectoryTrackingTruck(PathInfoType &path, geometry_msgs::msg::Pose2D &vehicle_center, double max_speed, double max_omg, double target_speed,double dec_new, double vehicle_speed, double &vel_cmd,double &w_cmd, double cycle_time,int stop_step,uint8_t stop_obs_flag,int turn_direction, double travel_dis, PIDControl&  pid_control_offsety,PIDControl& pid_control_heading,ControlParamType controlParamInfo)
    :path_(path), vehicle_center_(vehicle_center), max_speed_(max_speed), max_omg_(max_omg), target_speed_(target_speed),dec_new_(dec_new), vehicle_speed_(vehicle_speed), vel_cmd_(vel_cmd),w_cmd_(w_cmd),cycle_time_(cycle_time),stop_step_(stop_step),stop_obs_flag_(stop_obs_flag),turn_direction_(turn_direction), travel_dis_(travel_dis), pid_control_offsety_(pid_control_offsety), pid_control_heading_(pid_control_heading),controlParamInfo_(controlParamInfo)
{
    //for test
    double dt         = cycle_time_;
    complete_flag_    = false;
    double radius     = 0.0;
    double current_target_dis = 0.0;
    int index_nearest = 0;
    //计算横向路径偏差
    Pose_type start_point,end_point;
    std::pair<double,double> cmd_w;

    if (path_.PathType == "LINE")
    {
        start_point  = path_.PathPointInfo[0];
        end_point    = path_.PathPointInfo[1];
        projection_  = getProjectPoint2Line(start_point,end_point,vehicle_center_);
        CalculateOffset(start_point,end_point,vehicle_center_,projection_);
        start_point_ = start_point;
        end_point_   = end_point;
    }
    else if (path_.PathType == "BESSEL")
    {
        TrajectoryGenerate();//生成曲线轨迹
        ProjectionIntervalType ProjectionInterval_info = CalculateProjectionInterval();
        projection_     = ProjectionInterval_info.projection_point;
        CalculateOffset(ProjectionInterval_info.start_point,ProjectionInterval_info.end_point,vehicle_center_,projection_);
        start_point_    = ProjectionInterval_info.start_point;
        end_point_      = ProjectionInterval_info.end_point;
        offset_heading_ = wrapToPI(ProjectionInterval_info.end_point.heading - vehicle_center_.theta);//modify 20201210

        Pose_type p0    = path_.PathPointInfo[1];
        Pose_type p1    = path_.PathPointInfo[0];
        Pose_type p2    = path_.PathPointInfo[2];
        double S_vec    = (p1.x-p0.x)*(p2.y-p0.y)-(p1.y-p0.y)*(p2.x-p0.x);
        turn_direction_ = (S_vec > EPSILON ? 1 : (S_vec < -EPSILON ? -1 : 0));
        radius          = 1.0/ProjectionInterval_info.end_point.curvature;
        end_point       = path_.PathPointInfo[1];
        start_point     = curve_path_points_[int(curve_path_points_.size())-2];
        index_nearest   = ProjectionInterval_info.index_nearest;
    }
    else
    {
        //
    }
    if(w_cmd_ < EPSILON ) 
    {
        pid_control_offsety.reSet();
        pid_control_heading.reSet();
    }
    cmd_w             = PIDController(vehicle_center_, path_.PathPointInfo[1], offset_y_, offset_heading_, controlParamInfo_, dt, pid_control_offsety, pid_control_heading);
    w_offsety_        = cmd_w.first;
    w_offsetheading_  = cmd_w.second;
    double dis_target = sqrt(pow(vehicle_center_.x - end_point.x,2) + pow(vehicle_center_.y - end_point.y,2));
    double vec1_x     = start_point.x - projection_.x;
    double vec1_y     = start_point.y - projection_.y;
    double vec2_x     = end_point.x   - projection_.x;
    double vec2_y     = end_point.y   - projection_.y;
    double vec1_dis   = sqrt(vec1_x * vec1_x + vec1_y * vec1_y);
    double vec2_dis   = sqrt(vec2_x * vec2_x + vec2_y * vec2_y);
    cos_theta_        = (vec1_x*vec2_x + vec1_y*vec2_y)/(vec1_dis*vec2_dis);
    double theta      = wrapToPI(acos(cos_theta_));
    if(cos_theta_ < -1.0 + EPSILON && cos_theta_ > -1.0 - EPSILON )
    {
        theta = PI;
    }
    if(cos_theta_ < 1.0 + EPSILON && cos_theta_ > 0.9 - EPSILON )
    {
        theta = 0.0;
    }
    // RCLCPP_WARN(node->get_logger(),"TrajectoryTracking:  stop_obs_flag_ = %d,vec1_dis = %f,vec2_dis = %f,cos_theta_ = %f,theta = %f,vel_cmd_ = %f,dis_target = %f,offset_heading_ = %f",\
    // stop_obs_flag_,vec1_dis,vec2_dis,cos_theta_,theta,vel_cmd_,dis_target,offset_heading_);
    if(stop_obs_flag_ == ESTOP)
    {
        w_cmd_     = 0.0;
	    if(vel_cmd_ > controlParamInfo_.estop_speed_level + EPSILON)
        {
	      vel_cmd_ = vel_cmd_ - controlParamInfo_.obs_estop_dec  * cycle_time_;
          vel_cmd_ = vel_cmd_ <= controlParamInfo_.estop_speed_level ? controlParamInfo_.estop_speed_level : vel_cmd_;
        }
        else
        {
	      vel_cmd_ = vel_cmd_ - controlParamInfo_.obs_brake_dec  * cycle_time_;
	      vel_cmd_ = vel_cmd_ < 0.05 ? 0.0 : vel_cmd_;
	    }
    }


    else
    {
        double new_break_vel_cmd = vel_cmd_;

        if (path_.PathType == "BESSEL")
        {
            current_target_dis = CalculateCurveLength(index_nearest,curve_path_points_,start_point, end_point,vehicle_center_);
            if(fabs(theta)<controlParamInfo_.offsetangle_tolerance && vec1_dis > vec2_dis )
            {
                current_target_dis = 0.0;
            }
            vel_cmd_ = VelocityPlan(path_.PathType,current_target_dis,path_.PathPointInfo[1], max_speed_,target_speed_,vel_cmd_,stop_step_);
            w_cmd_   = turn_direction_ * vel_cmd_/radius;
            w_cmd_   = w_cmd_ + cmd_w.first + cmd_w.second;
            w_cmd_   = (w_cmd_>max_omg_ + 0.5 ? max_omg_ + 0.5 : (w_cmd_<-max_omg_-0.5 ? -max_omg_-0.5 : w_cmd_));
        }
        else
        {
            //current_target_dis = sqrt(pow(vehicle_center_.x - end_point.x,2) + pow(vehicle_center_.y - end_point.y,2));//distance
            current_target_dis = sqrt(pow(projection_.x - end_point.x,2) + pow(projection_.y - end_point.y,2));//distance
            vel_cmd_ = VelocityPlan(path_.PathType,current_target_dis,path_.PathPointInfo[1], max_speed_,target_speed_,vel_cmd_,stop_step_);
            w_cmd_   = cmd_w.first + cmd_w.second;
            w_cmd_   = (w_cmd_ > max_omg_ ? max_omg_ : (w_cmd_ < -max_omg_ ? -max_omg_ : w_cmd_));//将w_cmd_进行限制(-0.5,0.5)rad/s
        }

        if(stop_obs_flag_ == BRAKE)
        {
            new_break_vel_cmd = new_break_vel_cmd - controlParamInfo_.obs_brake_dec  * cycle_time_;
            double target_break_speed = max_speed_ < 0.3 ? max_speed_ : 0.3;
            new_break_vel_cmd = new_break_vel_cmd < target_break_speed ? target_break_speed : new_break_vel_cmd;
            if(new_break_vel_cmd < vel_cmd_){   
                vel_cmd_ = new_break_vel_cmd;
                if (path_.PathType == "BESSEL")
                {
                    w_cmd_ = turn_direction_ * new_break_vel_cmd/radius;
                    w_cmd_ = w_cmd_ + cmd_w.first + cmd_w.second;
                    w_cmd_ = (w_cmd_ > max_omg_ + 0.8 ? max_omg_ + 0.8 : (w_cmd_ < -max_omg_-0.8 ? -max_omg_-0.8 : w_cmd_));
                }
                else
                {
                    w_cmd_ = cmd_w.first + cmd_w.second;
                    w_cmd_ = (w_cmd_ > max_omg_ ? max_omg_ : (w_cmd_ < -max_omg_ ? -max_omg_ : w_cmd_));//将w_cmd_进行限制(-0.5,0.5)rad/s
                }
            }
        }
  
        //RCLCPP_WARN(node->get_logger(),"TrajectoryTracking:  target_speed_ = %f, vec1_dis = %f,vec2_dis = %f,cos_theta_ = %f,theta = %f,vel_cmd_ = %f,dis_target = %f,offset_heading_ = %f",target_speed_,vec1_dis,vec2_dis,cos_theta_,theta,vel_cmd_,dis_target,offset_heading_);
        if((fabs(theta) <= PI + EPSILON && fabs(theta) >= PI-controlParamInfo_.offsetangle_tolerance && vec1_dis >= vec2_dis && vec2_dis < controlParamInfo_.arrive_target_tolerance) || (fabs(theta)< controlParamInfo_.offsetangle_tolerance && vec1_dis > vec2_dis )|| (dis_target<controlParamInfo_.arrive_target_tolerance))        {
            if(vel_cmd_ > target_speed_)
                vel_cmd_     = target_speed_;
            w_cmd_       = 0.0;
            cmd_w.first  = 0.0;
            cmd_w.second = 0.0;
            // RCLCPP_WARN(node->get_logger(),"TrajectoryTracking-LINE 2 : vehicle_speed_ = %f,SPEED_EPSILON = %f",vehicle_speed_,SPEED_EPSILON);
            if(fabs(vehicle_speed_) < target_speed_ + SPEED_EPSILON)
            {
                complete_flag_ = true;
                RCLCPP_WARN(node->get_logger(),"TrajectoryTracking-LINE 2 : arrived current line target point.");
                RCLCPP_WARN(node->get_logger(),"TrajectoryTracking-LINE vehicle_center_.x:%f,vehicle_center_.y:%f,dis_target:%f.",vehicle_center_.x,vehicle_center_.y,dis_target);
                return;
            }
        }
        RCLCPP_WARN(node->get_logger(),"TrajectoryTracking-LINE vehicle_center_.x:%f,vehicle_center_.y:%f,target.x:%f,target.y:%f.",vehicle_center_.x,vehicle_center_.y,end_point.x,end_point.y);
        RCLCPP_WARN(node->get_logger(),"current_target_dis:%f:%f ", current_target_dis,vec2_dis);
        if(fabs(vel_cmd_) == 0)
        {
            w_cmd_ = 0.0;
        }

    }
    travel_dis_ = TravelDistance(path_.PathType, travel_dis_, fabs(offset_y_), vehicle_speed_, cycle_time_);
    //RCLCPP_WARN(node->get_logger(),"TrajectoryTracking : offset_y_ = %f, travel_dis_ = %f,vehicle_speed_ = %f",offset_y_,travel_dis_,vehicle_speed_);
}

void TrajectoryTrackingTruck::TrajectoryGenerate()
{
    Pose_type curve_start   = path_.PathPointInfo[0];
    Pose_type curve_end     = path_.PathPointInfo[1];
    Pose_type curve_control = path_.PathPointInfo[2];
    double dis_1            = sqrt((curve_start.x-curve_control.x)*(curve_start.x-curve_control.x)+(curve_start.y-curve_control.y)*(curve_start.y-curve_control.y));
    double dis_2            = sqrt((curve_end.x-curve_control.x)*(curve_end.x-curve_control.x)+(curve_end.y-curve_control.y)*(curve_end.y-curve_control.y));
    int insert_point_num    = int((dis_1 + dis_2)/controlParamInfo_.curve_insert_dis);
    control_point_          = curve_control;
    curve_path_points_.clear();
    double t_min            = 0.0;
    double t_max            = 1.0;
    double interval_value   = (t_max - t_min)/insert_point_num;
    for (double i = t_min; i <= t_max; i = i+interval_value)
    {
        Pose_type path_point;
        path_point.x  = (1-i)*(1-i)*curve_start.x + 2*i*(1-i)*curve_control.x + i*i*curve_end.x;
        path_point.y  = (1-i)*(1-i)*curve_start.y + 2*i*(1-i)*curve_control.y + i*i*curve_end.y;
        double dx_dt  = 2*(i-1)*curve_start.x + (2-4*i)*curve_control.x + 2*i*curve_end.x;
        double dy_dt  = 2*(i-1)*curve_start.y + (2-4*i)*curve_control.y + 2*i*curve_end.y;
        double ddx_dt = 2*curve_start.x -4*curve_control.x + 2*curve_end.x;
        double ddy_dt = 2*curve_start.y -4*curve_control.y + 2*curve_end.y;
        //参数方程的曲率求解公式
        double k_scaler_1 = dx_dt*dx_dt + dy_dt*dy_dt;
        double curvature  = fabs(dx_dt*ddy_dt - ddx_dt*dy_dt)/(sqrt(pow(k_scaler_1,3)));
        //calculate point's heading
        if(i == t_min)
        {
            path_point.heading = curve_start.heading;
        }
        else if(i == t_max)
        {
            path_point.heading = curve_end.heading;
        }
        else
        {
            if (dx_dt > EPSILON)
            {
                path_point.heading = dy_dt > EPSILON ? atan(dy_dt/dx_dt) : (dy_dt < -EPSILON ? atan(dy_dt/dx_dt) : 0.0);
            }
            else if(dx_dt < EPSILON)
            {
                path_point.heading = dy_dt > EPSILON ? PI - fabs(atan(dy_dt/dx_dt)) : (dy_dt < -EPSILON ? -PI + atan(dy_dt/dx_dt) : (dy_dt >= 0.0 ? PI : -PI));
            }
            else
            {
                path_point.heading = dy_dt > EPSILON ? 0.5 * PI : (dy_dt < -EPSILON ? -0.5 * PI : 0.0);
            }
        }
        //RCLCPP_INFO(node->get_logger(),"BESSEL: path_point.x = %f,path_point.y = %f,path_point.heading = %f",path_point.x,path_point.y,path_point.heading);
        path_point.curvature = curvature;
        curve_path_points_.emplace_back(path_point);
    }
}

ProjectionIntervalType TrajectoryTrackingTruck::CalculateProjectionInterval()
{
    ProjectionIntervalType projection_interval_info;
    Pose_type start_point, end_point;
    int index_nearest          = 0;
    double min_value           = MAX_NUM;
    int curve_path_point_count = int(curve_path_points_.size());
    for (size_t i = 0 ; i < curve_path_point_count ; i++)
    {
        Pose_type curvePoint = curve_path_points_[i];
        double distance      = sqrt((vehicle_center_.x - curvePoint.x)*(vehicle_center_.x - curvePoint.x)+(vehicle_center_.y - curvePoint.y)*(vehicle_center_.y - curvePoint.y));
        if (distance < min_value)
        {
            min_value     = distance;
            index_nearest = i;
        }
    }
    //确定投影点的区间[start_point,end_point],准确判断出投影点所在的区间
    Pose_type nearest_point = curve_path_points_[index_nearest];
    Pose_type last_point, next_point, projection_point;
    bool inside_left_flag = false,inside_right_flag = false;
    if(index_nearest == 0)
    {
        last_point       = curve_path_points_[index_nearest];
        next_point       = curve_path_points_[index_nearest+1];
        projection_point = getProjectPoint2Line(last_point, next_point, vehicle_center_);
        inside_left_flag = JudgeInterval(last_point,next_point, projection_point);
        if (inside_left_flag == true)
        {
            start_point = last_point;
            end_point   = next_point;
        }
        else
        {
            start_point = next_point;
            end_point   = curve_path_points_[index_nearest+2];
        }
    }
    else if(index_nearest == curve_path_point_count-1)
    {
        last_point = curve_path_points_[index_nearest-1];
        next_point = curve_path_points_[index_nearest];
        projection_point  = getProjectPoint2Line(last_point, next_point, vehicle_center_);
        inside_left_flag  = JudgeInterval(last_point,next_point, projection_point);
        if (inside_left_flag == true)
        {
            start_point = last_point;
            end_point   = next_point;
        }
        else
        {
            start_point = last_point;
            end_point   = next_point;
        }
    }
    else
    {
        last_point        = curve_path_points_[index_nearest-1];
        next_point        = curve_path_points_[index_nearest+1];
        projection_point  = getProjectPoint2Line(last_point, nearest_point, vehicle_center_);
        inside_left_flag  = JudgeInterval(last_point,nearest_point, projection_point);
        inside_right_flag = JudgeInterval(nearest_point,next_point, projection_point);
        if (inside_left_flag == true)
        {
            start_point  = last_point;
            end_point    = nearest_point;
        }
        else
        {
            if (inside_right_flag == true)
            {
                start_point = nearest_point;
                end_point   = next_point;
            }
            else
            {
                start_point = nearest_point;
                end_point   = next_point;
            }
        }
    }
    projection_interval_info.start_point      = start_point;
    projection_interval_info.end_point        = end_point;
    projection_interval_info.projection_point = projection_point;
    projection_interval_info.index_nearest    = index_nearest;

    return projection_interval_info;
}

double TrajectoryTrackingTruck::getLaneHeading(Pose_type p_start,Pose_type p_end)
{
    //计算直线的航向角
    double straightAngle = 0.0;
    double vec_x         = p_end.x - p_start.x;
    double vec_y         = p_end.y - p_start.y;
    double distance      = sqrt(vec_x*vec_x + vec_y*vec_y);
    double cos_theta     = vec_x/distance;
    // 4 quadrant direction angle
    if(vec_x >= EPSILON)
    {
        straightAngle = vec_y >= EPSILON ? acos(cos_theta) : (vec_y <= -EPSILON ? -acos(cos_theta) : 0.0);
    }
    else if(vec_x <= -EPSILON)
    {
        straightAngle = vec_y >= EPSILON ? acos(cos_theta) : (vec_y <= -EPSILON ? -acos(cos_theta) : (vec_y >= 0.0 ? PI : -PI));
    }
    else
    {
        straightAngle = vec_y >= EPSILON ? 0.5*PI : (vec_y <= -EPSILON ? -0.5*PI : 0.0);
    }
    straightAngle = wrapToPI(straightAngle);
    return straightAngle;
}

Pose_type TrajectoryTrackingTruck::getProjectPoint2Line(Pose_type &p1, Pose_type &p2, geometry_msgs::msg::Pose2D &p0)
{
    double vec_1_x       = p0.x - p1.x;
    double vec_1_y       = p0.y - p1.y;
    double vec_2_x       = p2.x - p1.x;
    double vec_2_y       = p2.y - p1.y;
    double vec1_dot_vec2 = vec_1_x * vec_2_x + vec_1_y * vec_2_y;
    double dis_start_end = vec_2_x * vec_2_x + vec_2_y * vec_2_y;
    double k_scaler      = vec1_dot_vec2/dis_start_end;
    double projection_x  = k_scaler * vec_2_x + p1.x;
    double projection_y  = k_scaler * vec_2_y + p1.y;

    Pose_type projectionPoint;
    projectionPoint.x    = projection_x;
    projectionPoint.y    = projection_y;

    return projectionPoint;
}

void TrajectoryTrackingTruck::CalculateOffset(Pose_type &p1, Pose_type &p2, geometry_msgs::msg::Pose2D &p0,Pose_type &projection_point)
{
    /*
    已知直线的起点p1和终点坐标p2
    已知车辆的当前点坐标p0
    应用向量法求出当前点在直线上的投影点坐标以及路径偏差
    判断路径偏差的正负号，在车身的左侧还是右侧（左侧为正，右侧为负）
    */
    double offset_y      = sqrt((p0.x - projection_point.x) * (p0.x - projection_point.x) + (p0.y - projection_point.y)*(p0.y - projection_point.y));
    double S_vec         = (p1.x-p0.x)*(p2.y-p0.y)-(p1.y-p0.y)*(p2.x-p0.x);//计算车辆中心点在直线路径的左侧还是右侧
    int direction_flag   = 0;
    direction_flag       = (S_vec > EPSILON ? -1 : (S_vec < -EPSILON ? 1 : 0));
    offset_y_            = direction_flag * offset_y;//车身坐标系：直线在车身左侧则路径偏差为正，直线在车身右侧则路径偏差为负
    offset_x_            = sqrt((p2.x - projection_point.x) * (p2.x - projection_point.x) + (p2.y - projection_point.y)*(p2.y - projection_point.y));
    //计算角度的偏差，即车辆当前的航向角与直线路径的航向角的偏差
    double straightAngle = getLaneHeading(p1,p2);
    offset_heading_      = straightAngle - p0.theta;//目标heading-车辆当前heading
    offset_heading_      = wrapToPI(offset_heading_);
    RCLCPP_WARN(node->get_logger(),"offset_y_:%f",offset_y_);

}

double TrajectoryTrackingTruck::OmgCommandPlan_new(double init_angle, double current_angle, double target_angle, double omg_max, double &omg_command,uint8_t stop_obs_flag)
{
    double w_acc         = 0.785*0.25;//0.25*PI = 0.785,0.785*0.5
    double w_cycle_time  = 0.1;
    double w_max         = omg_max;
    double stop_angle_tolerance = 1.0*PI/180.0;
    double acc_angle_max = 0.5*w_max * w_max/w_acc;
    double dec_angle_max = 0.5*w_max * w_max/w_acc;
    double acc_angle_cur = 0.5*(w_max * w_max - omg_command * omg_command)/w_acc;
    double dec_angle_cur = 0.5*omg_command * omg_command/w_acc;
    // RCLCPP_ERROR(node->get_logger(),"OmgCommandPlan_new: stop_obs_flag = %d,init_angle = %f,current_angle = %f,target_angle = %f,acc_angle_max = %f,acc_angle_cur = %f,dec_angle_max = %f,dec_angle_cur = %f,omg_command = %f",
    //           int(stop_obs_flag),init_angle,current_angle,target_angle,acc_angle_max,acc_angle_cur,dec_angle_max,dec_angle_cur,omg_command);

    if(stop_obs_flag == 1)
    {
        w_cmd_ = 0.0;
        return w_cmd_;
    }
    else
    {
        //
    }

    if (target_angle >= 0.0 & target_angle < PI + EPSILON)
    {
        double theta_1 = target_angle - PI;
        if (init_angle >= 0.0 && init_angle < PI + EPSILON)
        {
            if (target_angle > init_angle)
            {
                double delta_theta_current_tatget = target_angle - current_angle;
                if(delta_theta_current_tatget > acc_angle_max + dec_angle_max)
                {
                    omg_command = omg_command + w_acc * w_cycle_time;
                    omg_command = omg_command > w_max ? w_max : omg_command;
                }
                else if(delta_theta_current_tatget > acc_angle_cur + dec_angle_max  && delta_theta_current_tatget <= acc_angle_max + dec_angle_max)
                {
                    double target_omg = 0.0;
                    double k1    = target_omg * target_omg / w_acc + omg_command * omg_command / w_acc;
                    double k2    = 1.0/w_acc + 1.0/w_acc;
                    double omg_1 = sqrt((2*delta_theta_current_tatget + k1)/k2);
                    omg_1        = omg_1 > w_max ? w_max : omg_1;
                    omg_command  = omg_command + w_acc * w_cycle_time;
                    omg_command  = omg_command > omg_1 ? omg_1 : omg_command;
                }
                else if(delta_theta_current_tatget <= acc_angle_cur + dec_angle_max && delta_theta_current_tatget > stop_angle_tolerance)
                {
                    double target_omg = 0.0;
                    double k1    = target_omg * target_omg / w_acc + omg_command * omg_command / w_acc;
                    double k2    = 1.0/w_acc + 1.0/w_acc;
                    double omg_1 = sqrt((2*delta_theta_current_tatget + k1)/k2);
                    omg_1        = omg_1>w_max ? w_max : omg_1;
                    if(delta_theta_current_tatget > dec_angle_cur + stop_angle_tolerance)
                    {
                        omg_command = omg_command + w_acc * w_cycle_time;
                        omg_command = omg_command > omg_1 ? omg_1 : omg_command;
                    }
                    else
                    {
                        omg_command = omg_command - w_acc * w_cycle_time;
                        omg_command = omg_command < target_omg  + 0.05 ? target_omg + 0.05 : omg_command;
                    }
                }
                else if(delta_theta_current_tatget <= stop_angle_tolerance && delta_theta_current_tatget > EPSILON)
                {
                    omg_command = 0.0;
                }
                else
                {
                    omg_command = -0.05;
                }
            }
            else
            {
                double delta_theta_current_tatget_1 = current_angle - target_angle;
                if(delta_theta_current_tatget_1 > acc_angle_max + dec_angle_max)
                {
                    omg_command = omg_command - w_acc * w_cycle_time;
                    omg_command = omg_command < -w_max ? -w_max : omg_command;
                }
                else if(delta_theta_current_tatget_1 > acc_angle_cur + dec_angle_max  && delta_theta_current_tatget_1 <= acc_angle_max + dec_angle_max)
                {
                    double target_omg = 0.0;
                    double k1    = target_omg * target_omg / w_acc + omg_command * omg_command / w_acc;
                    double k2    = 1.0/w_acc + 1.0/w_acc;
                    double omg_1 = sqrt((2*delta_theta_current_tatget_1 + k1)/k2);
                    omg_1        = omg_1>w_max ? w_max : omg_1;
                    omg_command  = omg_command - w_acc * w_cycle_time;
                    omg_command  = omg_command < -omg_1 ? -omg_1 : omg_command;
                }
                else if(delta_theta_current_tatget_1 <= acc_angle_cur + dec_angle_max && delta_theta_current_tatget_1 > stop_angle_tolerance)
                {
                    double target_omg = 0.0;
                    double k1    = target_omg * target_omg / w_acc + omg_command * omg_command / w_acc;
                    double k2    = 1.0/w_acc + 1.0/w_acc;
                    double omg_1 = sqrt((2*delta_theta_current_tatget_1 + k1)/k2);
                    omg_1        = omg_1>w_max ? w_max : omg_1;
                    if(delta_theta_current_tatget_1 > dec_angle_cur + stop_angle_tolerance)
                    {
                        omg_command = omg_command - w_acc * w_cycle_time;
                        omg_command = omg_command < -omg_1 ? -omg_1 : omg_command;
                    }
                    else
                    {
                        omg_command = omg_command + w_acc * w_cycle_time;
                        omg_command = omg_command > target_omg  - 0.05 ? target_omg - 0.05 : omg_command;
                    }
                }
                else if(delta_theta_current_tatget_1 <= stop_angle_tolerance && delta_theta_current_tatget_1 > EPSILON)
                {
                    omg_command = 0.0;
                }
                else
                {
                    omg_command = 0.05;
                }
            }
        }
        else
        {
            if (init_angle > theta_1)
            {
                double delta_theta_current_tatget_2 = target_angle - current_angle;
                if(delta_theta_current_tatget_2 > acc_angle_max + dec_angle_max)
                {
                    omg_command = omg_command + w_acc * w_cycle_time;
                    omg_command = omg_command>w_max ? w_max : omg_command;
                }
                else if(delta_theta_current_tatget_2 > acc_angle_cur + dec_angle_max  && delta_theta_current_tatget_2 <= acc_angle_max + dec_angle_max)
                {
                    double target_omg = 0.0;
                    double k1    = target_omg * target_omg / w_acc + omg_command * omg_command / w_acc;
                    double k2    = 1.0/w_acc + 1.0/w_acc;
                    double omg_1 = sqrt((2*delta_theta_current_tatget_2 + k1)/k2);
                    omg_1        = omg_1>w_max ? w_max : omg_1;
                    omg_command  = omg_command + w_acc * w_cycle_time;
                    omg_command  = omg_command > omg_1 ? omg_1 : omg_command;
                }
                else if(delta_theta_current_tatget_2 <= acc_angle_cur + dec_angle_max && delta_theta_current_tatget_2 > stop_angle_tolerance)
                {
                    double target_omg = 0.0;
                    double k1    = target_omg * target_omg / w_acc + omg_command * omg_command / w_acc;
                    double k2    = 1.0/w_acc + 1.0/w_acc;
                    double omg_1 = sqrt((2*delta_theta_current_tatget_2 + k1)/k2);
                    omg_1        = omg_1>w_max ? w_max : omg_1;
                    if(delta_theta_current_tatget_2 > dec_angle_cur + stop_angle_tolerance)
                    {
                        omg_command = omg_command + w_acc * w_cycle_time;
                        omg_command = omg_command > omg_1 ? omg_1 : omg_command;
                    }
                    else
                    {
                        omg_command = omg_command - w_acc * w_cycle_time;
                        omg_command = omg_command < target_omg  + 0.05 ? target_omg + 0.05 : omg_command;
                    }
                }
                else if(delta_theta_current_tatget_2 <= stop_angle_tolerance && delta_theta_current_tatget_2 > EPSILON)
                {
                    omg_command = 0.0;
                }
                else
                {
                    omg_command = -0.05;
                }
            }
            else
            {
                double delta_theta_current_tatget_3 = 2.0*PI - (target_angle - current_angle);
                if((current_angle >= -PI - EPSILON && current_angle <= init_angle ) || (current_angle > init_angle && current_angle <= 0.0))
                {
                    delta_theta_current_tatget_3 = 2.0*PI - (target_angle - current_angle);
                }
                else if (current_angle <= PI + EPSILON && current_angle >= target_angle)
                {
                    delta_theta_current_tatget_3 = current_angle - target_angle;
                }
                else
                {
                    delta_theta_current_tatget_3 = current_angle - target_angle;
                }

                if(delta_theta_current_tatget_3 > acc_angle_max + dec_angle_max)
                {
                    omg_command = omg_command - w_acc * w_cycle_time;
                    omg_command = omg_command < -w_max ? -w_max : omg_command;
                }
                else if(delta_theta_current_tatget_3 > acc_angle_cur + dec_angle_max  && delta_theta_current_tatget_3 <= acc_angle_max + dec_angle_max)
                {
                    double target_omg = 0.0;
                    double k1    = target_omg * target_omg / w_acc + omg_command * omg_command / w_acc;
                    double k2    = 1.0/w_acc + 1.0/w_acc;
                    double omg_1 = sqrt((2*delta_theta_current_tatget_3 + k1)/k2);
                    omg_command  = omg_command - w_acc * w_cycle_time;
                    omg_command  = omg_command < -omg_1 ? -omg_1 : omg_command;
                }
                else if(delta_theta_current_tatget_3 <= acc_angle_cur + dec_angle_max && delta_theta_current_tatget_3 > stop_angle_tolerance)
                {
                    double target_omg = 0.0;
                    double k1         = target_omg * target_omg / w_acc + omg_command * omg_command / w_acc;
                    double k2         = 1.0/w_acc + 1.0/w_acc;
                    double omg_1      = sqrt((2*delta_theta_current_tatget_3 + k1)/k2);
                    if(delta_theta_current_tatget_3 > dec_angle_cur + stop_angle_tolerance)
                    {
                        omg_command = omg_command - w_acc * w_cycle_time;
                        omg_command = omg_command < -omg_1 ? -omg_1 : omg_command;
                    }
                    else
                    {
                        omg_command = omg_command + w_acc * w_cycle_time;
                        omg_command = omg_command > target_omg  - 0.05 ? target_omg - 0.05 : omg_command;
                    }
                }
                else if(delta_theta_current_tatget_3 <= stop_angle_tolerance && delta_theta_current_tatget_3 > EPSILON)
                {
                    omg_command = 0.0;
                }
                else
                {
                    omg_command = 0.05;
                }
            }
        }
    }
    else
    {
        double theta_2 = target_angle + PI;
        if (init_angle >= -PI-EPSILON && init_angle < 0.0)
        {
            if (init_angle > target_angle)
            {
                double delta_theta_current_tatget_4 = current_angle - target_angle;
                if(delta_theta_current_tatget_4 > acc_angle_max + dec_angle_max)
                {
                    omg_command = omg_command - w_acc * w_cycle_time;
                    omg_command = omg_command < -w_max ? -w_max : omg_command;
                }
                else if(delta_theta_current_tatget_4 > acc_angle_cur + dec_angle_max  && delta_theta_current_tatget_4 <= acc_angle_max + dec_angle_max)
                {
                    double target_omg = 0.0;
                    double k1 = target_omg * target_omg / w_acc + omg_command * omg_command / w_acc;
                    double k2 = 1.0/w_acc + 1.0/w_acc;
                    double omg_1 = sqrt((2*delta_theta_current_tatget_4 + k1)/k2);
                    omg_1        = omg_1>w_max ? w_max : omg_1;
                    omg_command  = omg_command - w_acc * w_cycle_time;
                    omg_command  = omg_command < -omg_1 ? -omg_1 : omg_command;
                }
                else if(delta_theta_current_tatget_4 <= acc_angle_cur + dec_angle_max && delta_theta_current_tatget_4 > stop_angle_tolerance)
                {
                    double target_omg = 0.0;
                    double k1    = target_omg * target_omg / w_acc + omg_command * omg_command / w_acc;
                    double k2    = 1.0/w_acc + 1.0/w_acc;
                    double omg_1 = sqrt((2*delta_theta_current_tatget_4 + k1)/k2);
                    omg_1        = omg_1>w_max ? w_max : omg_1;
                    if(delta_theta_current_tatget_4 > dec_angle_cur + stop_angle_tolerance)
                    {
                        omg_command = omg_command - w_acc * w_cycle_time;
                        omg_command = omg_command < -omg_1 ? -omg_1 : omg_command;
                    }
                    else
                    {
                        omg_command = omg_command + w_acc * w_cycle_time;
                        omg_command = omg_command > target_omg  - 0.05 ? target_omg - 0.05 : omg_command;
                    }
                }
                else if(delta_theta_current_tatget_4 <= stop_angle_tolerance && delta_theta_current_tatget_4 > EPSILON)
                {
                    omg_command = 0.0;
                }
                else
                {
                    omg_command = 0.05;
                }
            }
            else
            {
                double delta_theta_current_tatget_5 = target_angle - current_angle;

                if(delta_theta_current_tatget_5 > acc_angle_max + dec_angle_max)
                {
                    omg_command = omg_command + w_acc * w_cycle_time;
                    omg_command = omg_command > w_max ? w_max : omg_command;
                }
                else if(delta_theta_current_tatget_5 > acc_angle_cur + dec_angle_max  && delta_theta_current_tatget_5 <= acc_angle_max + dec_angle_max)
                {
                    double target_omg = 0.0;
                    double k1 = target_omg * target_omg / w_acc + omg_command * omg_command / w_acc;
                    double k2 = 1.0/w_acc + 1.0/w_acc;
                    double omg_1 = sqrt((2*delta_theta_current_tatget_5 + k1)/k2);
                    omg_1        = omg_1>w_max ? w_max : omg_1;
                    omg_command  = omg_command + w_acc * w_cycle_time;
                    omg_command  = omg_command > omg_1 ? omg_1 : omg_command;
                }
                else if(delta_theta_current_tatget_5 <= acc_angle_cur + dec_angle_max && delta_theta_current_tatget_5 > stop_angle_tolerance)
                {
                    double target_omg = 0.0;
                    double k1    = target_omg * target_omg / w_acc + omg_command * omg_command / w_acc;
                    double k2    = 1.0/w_acc + 1.0/w_acc;
                    double omg_1 = sqrt((2*delta_theta_current_tatget_5 + k1)/k2);
                    omg_1        = omg_1>w_max ? w_max : omg_1;
                    if(delta_theta_current_tatget_5 > dec_angle_cur + stop_angle_tolerance)
                    {
                        omg_command = omg_command + w_acc * w_cycle_time;
                        omg_command = omg_command > omg_1 ? omg_1 : omg_command;
                    }
                    else
                    {
                        omg_command = omg_command - w_acc * w_cycle_time;
                        omg_command = omg_command < target_omg  + 0.05 ? target_omg + 0.05 : omg_command;
                    }
                }
                else if(delta_theta_current_tatget_5 <= stop_angle_tolerance && delta_theta_current_tatget_5 > EPSILON)
                {
                    omg_command = 0.0;
                }
                else
                {
                    omg_command = -0.05;
                }
            }
        }
        else
        {
            if (init_angle < theta_2)
            {
                double delta_theta_current_tatget_6 = current_angle - target_angle;
                if(delta_theta_current_tatget_6 > acc_angle_max + dec_angle_max)
                {
                    omg_command = omg_command - w_acc * w_cycle_time;
                    omg_command = omg_command < -w_max ? -w_max : omg_command;
                }
                else if(delta_theta_current_tatget_6 > acc_angle_cur + dec_angle_max  && delta_theta_current_tatget_6 <= acc_angle_max + dec_angle_max)
                {
                    double target_omg = 0.0;
                    double k1    = target_omg * target_omg / w_acc + omg_command * omg_command / w_acc;
                    double k2    = 1.0/w_acc + 1.0/w_acc;
                    double omg_1 = sqrt((2*delta_theta_current_tatget_6 + k1)/k2);
                    omg_1        = omg_1>w_max ? w_max : omg_1;
                    omg_command  = omg_command - w_acc * w_cycle_time;
                    omg_command  = omg_command < -omg_1 ? -omg_1 : omg_command;
                }
                else if(delta_theta_current_tatget_6 <= acc_angle_cur + dec_angle_max && delta_theta_current_tatget_6 > stop_angle_tolerance)
                {
                    double target_omg = 0.0;
                    double k1 = target_omg * target_omg / w_acc + omg_command * omg_command / w_acc;
                    double k2 = 1.0/w_acc + 1.0/w_acc;
                    double omg_1 = sqrt((2*delta_theta_current_tatget_6 + k1)/k2);
                    omg_1 = omg_1>w_max ? w_max : omg_1;
                    if(delta_theta_current_tatget_6 > dec_angle_cur + stop_angle_tolerance)
                    {
                        omg_command = omg_command - w_acc * w_cycle_time;
                        omg_command = omg_command < -omg_1 ? -omg_1 : omg_command;
                    }
                    else
                    {
                        omg_command = omg_command + w_acc * w_cycle_time;
                        omg_command = omg_command > target_omg  - 0.05 ? target_omg - 0.05 : omg_command;
                    }
                }
                else if(delta_theta_current_tatget_6 <= stop_angle_tolerance && delta_theta_current_tatget_6 > EPSILON)
                {
                    omg_command = 0.0;
                }
                else
                {
                    omg_command = 0.05;
                }
            }
            else
            {
                double delta_theta_current_tatget_7 = 2.0*PI - (current_angle - target_angle);

                if(current_angle > 0.0 && current_angle <= PI + EPSILON)
                {
                    delta_theta_current_tatget_7 = 2.0*PI - (current_angle - target_angle);
                }
                else
                {
                    if(current_angle >= -PI-EPSILON && current_angle <= target_angle )
                    {
                        delta_theta_current_tatget_7 = -(current_angle - target_angle);
                    }
                    else
                    {
                        delta_theta_current_tatget_7 = target_angle - current_angle;
                    }
                }
                if(delta_theta_current_tatget_7 > acc_angle_max + dec_angle_max)
                {
                    omg_command = omg_command + w_acc * w_cycle_time;
                    omg_command = omg_command > w_max ? w_max : omg_command;
                }
                else if(delta_theta_current_tatget_7 > acc_angle_cur + dec_angle_max  && delta_theta_current_tatget_7 <= acc_angle_max + dec_angle_max)
                {
                    double target_omg = 0.0;
                    double k1 = target_omg * target_omg / w_acc + omg_command * omg_command / w_acc;
                    double k2 = 1.0/w_acc + 1.0/w_acc;
                    double omg_1 = sqrt((2*delta_theta_current_tatget_7 + k1)/k2);
                    omg_1        = omg_1>w_max ? w_max : omg_1;
                    omg_command  = omg_command + w_acc * w_cycle_time;
                    omg_command  = omg_command > omg_1 ? omg_1 : omg_command;
                }
                else if(delta_theta_current_tatget_7 <= acc_angle_cur + dec_angle_max && delta_theta_current_tatget_7 > stop_angle_tolerance)
                {
                    double target_omg = 0.0;
                    double k1    = target_omg * target_omg / w_acc + omg_command * omg_command / w_acc;
                    double k2    = 1.0/w_acc + 1.0/w_acc;
                    double omg_1 = sqrt((2*delta_theta_current_tatget_7 + k1)/k2);
                    omg_1        = omg_1>w_max ? w_max : omg_1;
                    if(delta_theta_current_tatget_7 > dec_angle_cur + stop_angle_tolerance)
                    {
                        omg_command = omg_command + w_acc * w_cycle_time;
                        omg_command = omg_command > omg_1 ? omg_1 : omg_command;
                    }
                    else
                    {
                        omg_command = omg_command - w_acc * w_cycle_time;
                        omg_command = omg_command < target_omg  + 0.05 ? target_omg + 0.05 : omg_command;
                    }
                }
                else if(delta_theta_current_tatget_7 <= stop_angle_tolerance && delta_theta_current_tatget_7 > EPSILON)
                {
                    omg_command = 0.0;
                }
                else
                {
                    omg_command = -0.05;
                }
            }
        }
    }
    omg_command = fabs(target_angle - current_angle) <= stop_angle_tolerance ? 0.0 : omg_command;
    return omg_command;
}

double TrajectoryTrackingTruck::VelocityPlan(std::string path_type, const double &current_target_dis,Pose_type &target_position,double &max_speed, double &targetVelocity,double &vel_cmd,int &stop_step)
{
    //modify 20201023
    double max_acc_dis     = 0.5*(max_speed * max_speed)/controlParamInfo_.acc_limit;//从0开始加到最大速度为止，所走的最大加速距离s1
    double cur_acc_dis     = 0.5*(max_speed * max_speed - vel_cmd * vel_cmd)/controlParamInfo_.acc_limit;//以当前速度开始加到最大速度为止，所走的加速距离s2
    double cur_dec_dis     = 0.5*(vel_cmd * vel_cmd - (targetVelocity + controlParamInfo_.creep_speed) * (targetVelocity + controlParamInfo_.creep_speed))/controlParamInfo_.dec_limit;//以当前速度开始减速到0，所走的最大距离s3
    double max_dec_dis     = 0.5*(max_speed * max_speed - (targetVelocity + controlParamInfo_.creep_speed) * (targetVelocity + controlParamInfo_.creep_speed))/controlParamInfo_.dec_limit + controlParamInfo_.creep_distance;//以最大速度进行减速到0，所走的最长减速距离s4
    double target_dis      = current_target_dis;//distance
    double target_dis_real = sqrt(pow(vehicle_center_.x - target_position.x,2) + pow(vehicle_center_.y - target_position.y,2));//distance
    double vt              = 0.0;
    if (path_type == "LINE")
    {
        target_dis_real = target_dis;
    }
    // RCLCPP_INFO(node->get_logger(),"TrajectoryTracking-CurveVelocityPlan: target_dis = %f,max_acc_dis = %f,cur_acc_dis = %f,cur_dec_dis = %f,max_dec_dis = %f,vel_cmd = %f,stop_step = %d",
            //  target_dis,max_acc_dis,cur_acc_dis,cur_dec_dis,max_dec_dis,vel_cmd,stop_step);

    //根据不同的最大速度计算加减速周期时间
    if(max_speed <= 0.5 + EPSILON)
    {
        cycle_time_ = cycle_time_ + controlParamInfo_.actuator_delaytime;
    }
    else if(max_speed <= 1.0 + EPSILON && max_speed > 0.5 + EPSILON)
    {
        cycle_time_ = cycle_time_ + controlParamInfo_.actuator_delaytime_1;
    }
    else
    {
        cycle_time_ = cycle_time_ + controlParamInfo_.actuator_delaytime_2;
    }

	if (target_dis > max_acc_dis + max_dec_dis)
    {
        vt =  vel_cmd > controlParamInfo_.start_speed_limit ?  vel_cmd + controlParamInfo_.acc_limit * cycle_time_ : vel_cmd + controlParamInfo_.start_acc_limit * cycle_time_;
    }
    else if (target_dis > max_dec_dis + cur_acc_dis && target_dis <= max_acc_dis + max_dec_dis)
    {
        vt =  vel_cmd > controlParamInfo_.start_speed_limit ?  vel_cmd + controlParamInfo_.acc_limit * cycle_time_ : vel_cmd + controlParamInfo_.start_acc_limit * cycle_time_;
    }
    else if (target_dis > max_dec_dis && target_dis <= max_dec_dis + max_acc_dis)
    {
        double k1   = targetVelocity * targetVelocity / controlParamInfo_.dec_limit + vel_cmd * vel_cmd / controlParamInfo_.acc_limit;
        double k2   = 1.0/controlParamInfo_.acc_limit + 1.0/controlParamInfo_.dec_limit;
        double vt_1 = sqrt((2*target_dis + k1)/k2);
        vt_1        = vt_1 > max_speed_ ? max_speed_ : vt_1;
        vt          = vel_cmd > controlParamInfo_.start_speed_limit ?  vel_cmd + controlParamInfo_.acc_limit * cycle_time_ : vel_cmd + controlParamInfo_.start_acc_limit * cycle_time_;
        vt          = vt > vt_1 ? vt_1 : vt;
    }
    else if(target_dis > controlParamInfo_.creep_distance && target_dis <= max_dec_dis)
    {
        switch(stop_step)
        {
            case 0:
                {
                    if(target_dis >= cur_dec_dis + controlParamInfo_.creep_distance)
                    {
                        double k1   = targetVelocity * targetVelocity / controlParamInfo_.dec_limit + vel_cmd * vel_cmd / controlParamInfo_.acc_limit;
                        double k2   = 1.0/controlParamInfo_.acc_limit + 1.0/controlParamInfo_.dec_limit;
                        double vt_1 = sqrt((2*target_dis + k1)/k2);
                        vt_1        = vt_1 > max_speed_ ? max_speed_ : vt_1;
                        vt          =  vel_cmd > controlParamInfo_.start_speed_limit ?  vel_cmd + controlParamInfo_.acc_limit * cycle_time_ : vel_cmd + controlParamInfo_.start_acc_limit * cycle_time_;
                        vt          = vt > vt_1 ? vt_1 : vt;
                    }
                    else
                    {
                        double dis_new = (target_dis - controlParamInfo_.stop_dis_tolerance_straight > EPSILON ? target_dis - controlParamInfo_.stop_dis_tolerance_straight : target_dis);
                        if (path_type == "BESSEL")
                        {
                            dis_new = target_dis > controlParamInfo_.stop_dis_tolerance_curve ? target_dis - controlParamInfo_.stop_dis_tolerance_curve : target_dis;
                        }
                        if (targetVelocity < EPSILON)
                        {
                            dec_new_ = fabs(0.5*(vel_cmd_ * vel_cmd_ - (targetVelocity + controlParamInfo_.creep_speed) * (targetVelocity + controlParamInfo_.creep_speed))/dis_new);
                            //20210310 add by byf
                            dec_new_ = dec_new_ > 0.5 ? 0.5 : dec_new_;
                            vt       = vel_cmd_ - dec_new_ * cycle_time_;
                            vt       = vt < targetVelocity + controlParamInfo_.creep_speed ? targetVelocity + controlParamInfo_.creep_speed : vt;
                        }
                        else
                        {
                            dis_new  = target_dis;
                            dec_new_ = fabs(0.5*(vel_cmd_ * vel_cmd_ - targetVelocity * targetVelocity)/dis_new);
                            vt       = vel_cmd_ - dec_new_ * cycle_time_;
                            vt       = vt < targetVelocity ? targetVelocity : vt;
                        }
                        stop_step = 1;
                        RCLCPP_ERROR(node->get_logger(),"curve stop 0 .dec_new_ = %f,stop_step = %d,target_dis = %f,is_new = %f",dec_new_,stop_step,target_dis,dis_new);
                    }
                }
                break;
            case 1:
                {
                    vt = vel_cmd_ - dec_new_ * cycle_time_;
                    //modify 20210309
                    if (targetVelocity < EPSILON)
                    {
                        vt = vt < targetVelocity + controlParamInfo_.creep_speed ? targetVelocity + controlParamInfo_.creep_speed : vt;
                    }
                    else
                    {
                        vt = vt < targetVelocity ? targetVelocity : vt;
                    }
                }
                break;
            default:
                break;
        }
    }
    // else if (target_dis >= controlParamInfo_.arrive_target_tolerance && target_dis <= controlParamInfo_.creep_distance)
    else if (target_dis <= controlParamInfo_.creep_distance)
    {
        if (targetVelocity < EPSILON)
        {
            vt = targetVelocity + controlParamInfo_.creep_speed;
            if(target_dis_real < controlParamInfo_.arrive_target_tolerance)
            {
                stop_step = 0;
                dec_new_  = 0.0;
                vt        = targetVelocity;
            }
        }
        else 
        {
            if(fabs(vel_cmd - targetVelocity) >0.05)
             vt =  vel_cmd > targetVelocity ?  vel_cmd - controlParamInfo_.dec_limit * cycle_time_ : vel_cmd + controlParamInfo_.acc_limit * cycle_time_;
             else 
             vt = targetVelocity;
            if(target_dis_real < controlParamInfo_.arrive_target_tolerance)
            {
                stop_step = 0;
                dec_new_  = 0.0;
            }
            // if(vel_cmd > targetVelocity)
            // vt = vt < targetVelocity ? targetVelocity : vt;
        }
    }
    // else if (target_dis < controlParamInfo_.arrive_target_tolerance)
    // {
    //      //modify 20210310 by byf
    //     vt = targetVelocity + controlParamInfo_.creep_speed;
    //     if(target_dis_real < controlParamInfo_.arrive_target_tolerance)
    //     {
    //         stop_step = 0;
    //         dec_new_  = 0.0;
    //         vt        = targetVelocity;
    //     }
    // }
    else
    {
        //
    }
    vel_cmd     = vt;
    vel_cmd     = vel_cmd > max_speed_ ? max_speed_ : vel_cmd;
    target_dis_ = target_dis;
    return vel_cmd;
}

bool TrajectoryTrackingTruck::JudgeInterval(Pose_type &p1, Pose_type &p2, Pose_type &projection)
{
    double vec1_x     = p1.x - projection.x;
    double vec1_y     = p1.y - projection.y;
    double vec2_x     = p2.x - projection.x;
    double vec2_y     = p2.y - projection.y;
    double vec1_dis   = sqrt(vec1_x * vec1_x + vec1_y * vec1_y);
    double vec2_dis   = sqrt(vec2_x * vec2_x + vec2_y * vec2_y);
    double cos_theta  = (vec1_x*vec2_x + vec1_y*vec2_y)/(vec1_dis*vec2_dis);
    double theta      = wrapToPI(acos(cos_theta));
    bool inside_flag  = false;
    if (fabs(theta) < controlParamInfo_.offsetangle_tolerance && vec1_dis > vec2_dis)
    {
        inside_flag = false;
    }
    else if (fabs(theta) < PI + EPSILON && fabs(theta) > PI - controlParamInfo_.offsetangle_tolerance)
    {
        inside_flag = true;
    }
    else
    {
        //
    }
    return inside_flag;
}

double TrajectoryTrackingTruck::CalculateCurveLength(int path_index,std::vector<Pose_type> curve_path_points,Pose_type &p1, Pose_type &p2,geometry_msgs::msg::Pose2D &vehicle_center)
{
    /*
    1.找到最近的路径点index，
    2.然后，判断投影点位于哪个区间，两个区间，左区间和右区间
    */
    double curve_length          = 0.0;
    double curve_first_length    = 0.0;
    //得到最近点index之后，判断当前点的投影点所在的区间
    Pose_type nearest_point      = curve_path_points[path_index];
    Pose_type last_point         = curve_path_points[path_index];
    Pose_type next_point         = curve_path_points[path_index];
    Pose_type projection_point   = getProjectPoint2Line(last_point, nearest_point, vehicle_center);
    Pose_type projection_point_1 = getProjectPoint2Line(nearest_point, next_point, vehicle_center);
    bool inside_left_flag        = false;
    bool inside_right_flag       = false;
    if(path_index == 0)
    {
        next_point         = curve_path_points[path_index+1];
        projection_point_1 = getProjectPoint2Line(nearest_point, next_point, vehicle_center);
        inside_right_flag  = JudgeInterval(nearest_point, next_point, projection_point_1);
        if (inside_right_flag == true)
        {
            curve_first_length = sqrt(pow(projection_point_1.x - next_point.x,2) + pow(projection_point_1.y - next_point.y,2));
        }
        else
        {
            curve_first_length = sqrt(pow(projection_point_1.x - nearest_point.x,2) + pow(projection_point_1.y - nearest_point.y,2));
        }
    }
    else if(path_index == int(curve_path_points.size())-1)
    {
        last_point         = curve_path_points[path_index-1];
        next_point         = curve_path_points[path_index];
        projection_point   = getProjectPoint2Line(last_point, nearest_point, vehicle_center);
        inside_left_flag   = JudgeInterval(last_point, nearest_point, projection_point);
        if (inside_left_flag == true)
        {
            curve_first_length = sqrt(pow(projection_point.x - nearest_point.x,2) + pow(projection_point.y - nearest_point.y,2));
        }
        else
        {
            curve_first_length = 0.0;
        }
    }
    else
    {
        last_point         = curve_path_points[path_index-1];
        next_point         = curve_path_points[path_index+1];
        projection_point   = getProjectPoint2Line(last_point, nearest_point, vehicle_center);
        projection_point_1 = getProjectPoint2Line(nearest_point, next_point, vehicle_center);
        inside_left_flag   = JudgeInterval(last_point, nearest_point, projection_point);
        inside_right_flag  = JudgeInterval(nearest_point, next_point, projection_point_1);
        if (inside_left_flag == true)
        {
            curve_first_length = sqrt(pow(projection_point.x - nearest_point.x,2) + pow(projection_point.y - nearest_point.y,2));
        }
        else
        {
            if (inside_right_flag == true)
            {
                curve_first_length = sqrt(pow(projection_point_1.x - next_point.x,2) + pow(projection_point_1.y - next_point.y,2));
            }
            else
            {
                curve_first_length = 0.0;
            }
        }
    }
    path_index   = std::min(path_index+1,int(curve_path_points.size())-1);
    curve_length = curve_first_length;
    for(size_t i=path_index;i<=int(curve_path_points.size())-1;i++)
    {
        Pose_type start =  i<int(curve_path_points.size())-1 ? curve_path_points[i] : curve_path_points[i-1];
        Pose_type end   =  i<int(curve_path_points.size())-1 ? curve_path_points[i + 1] : curve_path_points[i];
        double dis      = sqrt(pow(start.x-end.x,2) + pow(start.y-end.y,2));
        if(path_index == int(curve_path_points.size())-1)
        {
            dis = 0.0;
        }
        curve_length = curve_length + dis;
    }
    //RCLCPP_WARN(node->get_logger(),"TrajectoryTracking-CalculateCurveLength : path_index = %d,curve_path_points.size = %d,curve_length = %f",
    //          path_index,int(curve_path_points.size()),curve_length);
    return curve_length;
}

double TrajectoryTrackingTruck::TravelDistance(std::string path_type,double travel_dis,double offset_y,double current_speed,double cycletime)
{
    if (path_type == "LINE")
    {
        if (offset_y > controlParamInfo_.line_offsety && offset_y < controlParamInfo_.line_offsety_max)
        {
            travel_dis = travel_dis + current_speed * cycletime;
        }
        else if (offset_y >= controlParamInfo_.line_offsety_max)
        {
            travel_dis = controlParamInfo_.travel_dis_max;
        }
        else
        {
            travel_dis = 0.0;
        }
    }
    else
    {
        if (offset_y > controlParamInfo_.curve_offsety && offset_y < controlParamInfo_.curve_offsety_max)
        {
            travel_dis += current_speed * cycletime;
        }
        else if (offset_y >= controlParamInfo_.curve_offsety_max)
        {
            travel_dis = controlParamInfo_.travel_dis_max;
        }
        else
        {
            travel_dis = 0.0;
        }
    }
    //RCLCPP_WARN(node->get_logger(),"TravelDistance : offset_y = %f, travel_dis = %f,current_speed = %f",offset_y,travel_dis,current_speed);
    return travel_dis;
}

std::pair<double,double> TrajectoryTrackingTruck::PIDController(geometry_msgs::msg::Pose2D &vehicle_center, Pose_type &target_position, double offset_y,double offset_heading,ControlParamType controlParamInfo,double dt,PIDControl& pid_control_offsety,PIDControl& pid_control_heading)
{
    //在到达目标点的最后30cm这段距离，要进行精细控制，控制参数调大，尽快消除横向偏差
    //两种方式来消除横向偏差：（1）调大转向停止角度（2）控制参数调大，增大旋转角速度命令
    //设计两个参数（1.停止角度，2.控制参数）验证效果
    std::pair<double,double> cmd_w;
    double w1 = 0.0,w2 = 0.0;
    PIDConf pid_conf_offsety,pid_conf_heading;
    double current_target_distance = sqrt(pow(vehicle_center.x - target_position.x,2) + pow(vehicle_center.y - target_position.y,2));
    if (path_.PathType == "LINE")
    {
        pid_conf_offsety.kp  = controlParamInfo.adaptive_kp_line*max_speed_/(max_speed_ + 0.3 + 1.02 * vehicle_speed_ * vehicle_speed_);
        pid_conf_offsety.ki  = controlParamInfo.adaptive_ki_line;
        pid_conf_offsety.kd  = controlParamInfo.adaptive_kd_line;
        pid_conf_heading.kp  = controlParamInfo.adaptive_kp_heading_line;
        pid_conf_heading.ki  = controlParamInfo.adaptive_ki_heading_line;
        pid_conf_heading.kd  = controlParamInfo.adaptive_kd_heading_line;
        if (current_target_distance < controlParamInfo.change_control_param_dis)
        {
            pid_conf_offsety.kp  = controlParamInfo.adaptive_kp_line * 1.5;
            // pid_conf_offsety.kp  = controlParamInfo.adaptive_kp_line*max_speed_/(0.5*max_speed_  + 0.5 * vehicle_speed_ * vehicle_speed_);
            pid_conf_heading.kp  = controlParamInfo.adaptive_kp_heading_line;
        }

    }
    else
    {
        Pose_type start      = path_.PathPointInfo[0];
        Pose_type end        = path_.PathPointInfo[1];
        Pose_type control    = path_.PathPointInfo[2];
        double theta_1       = getLaneHeading(control,start);
        double theta_2       = getLaneHeading(control,end);
        double delta_theta   = fabs(wrapToPI(theta_1 - theta_2));
        pid_conf_offsety.kp  = controlParamInfo_.adaptive_kp_curve*(PI/(delta_theta + PI))*max_speed_/(max_speed_ + 0.5 + 1.02*vehicle_speed_ * vehicle_speed_);
        pid_conf_offsety.ki  = controlParamInfo_.adaptive_ki_curve;
        pid_conf_offsety.kd  = controlParamInfo_.adaptive_kd_curve;
        pid_conf_heading.kp  = controlParamInfo.adaptive_kp_heading_curve;
        pid_conf_heading.ki  = controlParamInfo.adaptive_ki_heading_curve;
        pid_conf_heading.kd  = controlParamInfo.adaptive_kd_heading_curve;
        if (current_target_distance < controlParamInfo.change_control_param_dis)
        {
            pid_conf_offsety.kp  = controlParamInfo_.adaptive_kp_curve*1.5;
            pid_conf_heading.kp  = controlParamInfo.adaptive_kp_heading_curve;

        }
    }
    pid_control_offsety.setPID(pid_conf_offsety);
    pid_control_heading.setPID(pid_conf_heading);
    
    if (current_target_distance < controlParamInfo.change_control_param_dis)
    {
        w1 = pid_control_offsety.control(offset_y,dt);
        w2 = pid_control_heading.control(offset_heading,dt)/1.0;
        // if (offset_y > controlParamInfo.offsety_tolerance)
        // {
        //     w1 = pid_control_offsety.control(offset_y_,dt);
        //     if(offset_y > controlParamInfo.offsety_tolerance + controlParamInfo.offsety_control_level_2)
        //     {
        //         w1 = offset_heading < -controlParamInfo.offsetangle_tolerance - controlParamInfo.offsetangle_control_level_2*PI/180.0 ? 0.0 : w1;
        //     }
        //     else
        //     {
        //         w1 = offset_heading < -controlParamInfo.offsetangle_tolerance ? 0.0 : w1;
        //     }
        // }
        // else if(offset_y < -controlParamInfo.offsety_tolerance)
        // {
        //     w1 = pid_control_offsety.control(offset_y,dt);
        //     if(offset_y < -controlParamInfo.offsety_tolerance - controlParamInfo.offsety_control_level_2)
        //     {
        //         w1 = offset_heading > controlParamInfo.offsetangle_tolerance + controlParamInfo.offsetangle_control_level_2*PI/180.0 ? 0.0 : w1;
        //     }
        //     else
        //     {
        //         w1 = offset_heading > controlParamInfo.offsetangle_tolerance ? 0.0 : w1;
        //     }
        // }
        // else
        // {
        //     w2 = pid_control_heading.control(offset_heading,dt);
        //     w2 = fabs(offset_heading) < controlParamInfo.offsetangle_tolerance1 ? 0.0 : w2;
        // }
    }
    else
    {
        w1 = pid_control_offsety.control(offset_y,dt);
        w2 = pid_control_heading.control(offset_heading,dt);

        if(fabs(w1 + w2) < controlParamInfo.pid_tolerance)
        {
            w1 = 0;
            w2 = 0;
        }
        // printf("PIDController : offset_y = %f, pid_conf_offsety.kp = %f,w1 = %f\r\n",offset_y,pid_conf_offsety.kp,w1);
        // if (offset_y > controlParamInfo.offsety_tolerance)
        // {
        //     w1 = pid_control_offsety.control(offset_y_,dt);
        //     if(offset_y > controlParamInfo.offsety_tolerance + controlParamInfo.offsety_control_level_1)
        //     {
        //         w1 = offset_heading < -controlParamInfo.offsetangle_tolerance - controlParamInfo.offsetangle_control_level_1*PI/180.0 ? 0.0 : w1;
        //     }
        //     else
        //     {
        //         w1 = offset_heading < -controlParamInfo.offsetangle_tolerance ? 0.0 : w1;
        //     }
        //     //add by cayla
        //     double offset_heading_tmp = offset_heading + atan(offset_y_ / 1.5);
        //     w2 = pid_control_heading.control(offset_heading_tmp,dt);
        //     w2 = fabs(offset_heading_tmp) < controlParamInfo.offsetangle_tolerance1 ? 0.0 : w2;
        // }
        // else if(offset_y < -controlParamInfo.offsety_tolerance)
        // {
        //     w1 = pid_control_offsety.control(offset_y,dt);
        //     if(offset_y < -controlParamInfo.offsety_tolerance - controlParamInfo.offsety_control_level_1)
        //     {
        //         w1 = offset_heading > controlParamInfo.offsetangle_tolerance + controlParamInfo.offsetangle_control_level_1*PI/180.0 ? 0.0 : w1;
        //     }
        //     else
        //     {
        //         w1 = offset_heading > controlParamInfo.offsetangle_tolerance ? 0.0 : w1;
        //     }
        //     //add by cayla
        //     double offset_heading_tmp = offset_heading + atan(offset_y_ / 1.5);
        //     w2 = pid_control_heading.control(offset_heading_tmp,dt);
        //     w2 = fabs(offset_heading_tmp) < controlParamInfo.offsetangle_tolerance1 ? 0.0 : w2;
        // }
        // else
        // {
        //     w2 = pid_control_heading.control(offset_heading,dt);
        //     w2 = fabs(offset_heading) < controlParamInfo.offsetangle_tolerance1 ? 0.0 : w2;
        // }
    }
    cmd_w.first  = w1;
    cmd_w.second = w2;
    return cmd_w;
}

std::string TrajectoryTrackingTruck::PathType()
{
    return path_.PathType;
}

Pose_type TrajectoryTrackingTruck::PathStartPoint() const
{
    return start_point_;
}

Pose_type TrajectoryTrackingTruck::PathEndPoint() const
{
    return end_point_;
}

Pose_type TrajectoryTrackingTruck::PathControlPoint() const
{
    return control_point_;
}

Pose_type TrajectoryTrackingTruck::getProjectionPoint() const
{
    return projection_;
}

double TrajectoryTrackingTruck::getOffsetY() const
{
    return offset_y_;
}

double TrajectoryTrackingTruck::getOffsetX() const
{
    return offset_x_;
}

double TrajectoryTrackingTruck::getOffsetHeading() const
{
    return offset_heading_;
}

double TrajectoryTrackingTruck::getOffsetSpeed() const
{
    return offset_speed_;
}

double TrajectoryTrackingTruck::getVelCmd()
{
    return vel_cmd_;
}

double TrajectoryTrackingTruck::getWCmd()
{
    return w_cmd_;
}

double TrajectoryTrackingTruck::getW_offsety()
{
    return w_offsety_;
}

double TrajectoryTrackingTruck::getW_offsetheading()
{
    return w_offsetheading_;
}

double TrajectoryTrackingTruck::getTargetDis()
{
    return target_dis_;
}

bool TrajectoryTrackingTruck::TrajectoryTracking_complete() const
{
    return complete_flag_;
}

int TrajectoryTrackingTruck::getStopStep()
{
    return stop_step_;
}

double TrajectoryTrackingTruck::getDecNew() const
{
    return dec_new_;
}

double TrajectoryTrackingTruck::getTravelDistance() const
{
    return travel_dis_;
}

double TrajectoryTrackingTruck::getCosTheta() const
{
    return cos_theta_;
}

void TrajectoryTrackingTruck::Clear()
{
    start_point_.x         = 0.0;
    start_point_.y         = 0.0;
    start_point_.heading   = 0.0;
    end_point_.x           = 0.0;
    end_point_.y           = 0.0;
    end_point_.heading     = 0.0;
    control_point_.x       = 0.0;
    control_point_.y       = 0.0;
    control_point_.heading = 0.0;
    offset_y_              = 0.0;
    offset_x_              = 0.0;
    offset_heading_        = 0.0;
    offset_speed_          = 0.0;
    vel_cmd_               = 0.0;
    w_cmd_                 = 0.0;
    target_dis_            = 0.0;
    dec_new_               = 0.0;
    complete_flag_         = false;
    stop_step_             = 0;
}

}//end namespace


